#include <iostream>
#include <stdio.h>
#include "../inc/PointCloud.h"
#include "../inc/XYGrid.h"

using namespace std;

XYGrid::XYGrid()
{
	setNbFrameX(0);
	setNbFrameY(0);
	this->data.clear();
}

int XYGrid::getNbFrameX ()
{
	return nbFrameX;
}

int XYGrid::getNbFrameY ()
{
	return nbFrameY;
}

float XYGrid::getFrameWidth ()
{
	return frameWidth;
}

float XYGrid::getFrameHeight ()
{
	return frameHeight;
}

int XYGrid::getSizeOfFrameCloud(int _i, int _j)
{
	return data[_i][_j].getSizeOfCloud();
}

void XYGrid::setNbFrameX(int _nbFrame)
{
	nbFrameX = _nbFrame;
}

void XYGrid::setNbFrameY(int _nbFrame)
{
	nbFrameY = _nbFrame;
}

XYGrid::XYGrid( PointCloud _p, float _xMin, float _yMin, float _xMax, float _yMax, int _resX, int _resY )
{
	float width = _xMax - _xMin;		/* largeur du nuage de points */
	float height = _yMax - _yMin;		/* longueur du nuage de points */

	frameWidth = width / (float)_resX;
	frameHeight = height / (float)_resY;
	
	setNbFrameX(_resX);
	setNbFrameY(_resY);
	
	bot.setXYZ(_xMin, _yMin, 0);
	top.setXYZ(_xMax, _yMax, 0);
	
	/* Creation d'une grille vide */
	for ( int i = 0 ; i < _resX ; i++ )
	{
		for ( int j = 0 ; j < _resY ; j++ )
		{
			data[i].push_back( GridFrame ( Point ( _xMin + (i*frameWidth), _yMin + (j*frameHeight), 0, 0, 0, 0), Point ( _xMin + ((i+1)*frameWidth), _yMin + ((j+1)*frameHeight), 0, 0, 0, 0)) );
		}
	}
	
	
	/* remplissage de la grille */
	for ( int i = 0 ; i < _p.size() ; i++ )
	{
		Point p = _p.get(i);
		int j,k;
		
		/* On ramene le repere a (_xMin, _yMin) */
		p.setX( p.getX() - _xMin );
		p.setY( p.getY() - _yMin );
		
		/* On calcule l'indice de la case ou inserer ce point */
		j = (int)p.getX();
		k = (int)p.getY();
		
		/* On reinitialise le repere a l'origine de la scene */
		p.setX( p.getX() + _xMin );
		p.setY( p.getY() + _yMin );
		
		/* On insere ce point dans la case correspondante */
		data[j][k].addPointToCloud( p );
	}
}

void XYGrid::getGridFromCloud(PointCloud _p, float _xMin, float _yMin, float _xMax, float _yMax, int _resX, int _resY)
{
	float width = _xMax - _xMin;		/* largeur du nuage de points */
	float height = _yMax - _yMin;		/* longueur du nuage de points */

	frameWidth = width / (float)_resX;
	frameHeight = height / (float)_resY;
	
	setNbFrameX(_resX);
	setNbFrameY(_resY);
	
	bot.setXYZ(_xMin, _yMin, 0);
	top.setXYZ(_xMax, _yMax, 0);
	
	/* Creation d'une grille vide */
	for ( int i = 0 ; i < _resX ; i++ )
	{
		data.push_back( vector<GridFrame> () );
		for ( int j = 0 ; j < _resY ; j++ )
		{
			data[i]	.push_back( GridFrame ( Point ( _xMin + (i*frameWidth), _yMin + (j*frameHeight), 0, 0, 0, 0), Point ( _xMin + ((i+1)*frameWidth), _yMin + ((j+1)*frameHeight), 0, 0, 0, 0)) );
		}
	}
			
	/* remplissage de la grille */
	for ( int i = 0 ; i < _p.size() ; i++ )
	{
		Point p = _p.get(i);
		int j,k;
		
		/* On ramene le repere a (_xMin, _yMin) */
		p.setX( p.getX() - _xMin );
		p.setY( p.getY() - _yMin );
		
		/* On calcule l'indice de la case ou inserer ce point */
		j = (int)(p.getX()/frameWidth);
		k = (int)(p.getY()/frameHeight);
		
		if ( j == _resX )
			j = _resX -1;
		
		if ( k == _resY )
			k = _resY -1;
		
		/* On reinitialise le repere a l'origine de la scene */
		p.setX( p.getX() + _xMin );
		p.setY( p.getY() + _yMin );
		
		/* On insere ce point dans la case correspondante */
		data[j][k].addPointToCloud( p );
	}
}

void XYGrid::displayGrid()
{
	for ( int i = 0 ; i < this->getNbFrameX() ; i++ )
	{
		for ( int j = 0 ; j < this->getNbFrameY() ; j++ )
		{
			this->data[i][j].displayFrame(0);
		}
	}
}

void XYGrid::displayGrid( float _z )
{
	for ( int i = 0 ; i < this->getNbFrameX() ; i++ )
	{
		for ( int j = 0 ; j < this->getNbFrameY() ; j++ )
		{
			this->data[i][j].displayFrame( _z );
		}
	}
}

void XYGrid::displayIJCloud(int _i, int _j)
{
	data[_i][_j].displayFrameColor();
	data[_i][_j].displayCloud();
}

void XYGrid::displayIJCloud(int _i, int _j, float _z)
{
	data[_i][_j].displayFrameColor( _z, 1, 0, 0 );
	data[_i][_j].displayCloud();
}

void XYGrid::displayIJCloud(int _i, int _j, float _z, float _r, float _g, float _b)
{
	/*data[_i][_j].displayFrameColor( _z, 0.2, 0.2, 0.2 );*/
	data[_i][_j].displayCloud(_r,_g,_b);
}

void XYGrid::keepPointsBetween(int _i, int _j, float _zMin, float _zMax)
{
	this->data[_i][_j].keepPointsBetween ( _zMin, _zMax );
}

Point XYGrid::getFrameBotPoint(int _i, int _j)
{
	return this->data[_i][_j].getBotPoint();
}

Point XYGrid::getFrameTopPoint(int _i, int _j)
{
	return this->data[_i][_j].getTopPoint();
}

void XYGrid::getFrameCircles(vector<Circle> &_tabCircles, int _i, int _j)
{
	this->data[_i][_j].getCircles(_tabCircles);
}

void XYGrid::getFrameCircles(HoughSpace & _hs, float _rMax, int _i, int _j)
{
	this->data[_i][_j].getCircles(_hs, _rMax);
}

void XYGrid::getFrameCircles(HoughSpace & _hs, float _rMax, int _i, int _j, int _coef)
{
	this->data[_i][_j].getCircles(_hs, _rMax, _coef);
}

void XYGrid::getAllCircles(vector<Circle> &_tabCircles)
{
	for ( int i = 0 ; i < nbFrameX ; i++ )
		for ( int j = 0 ; j < nbFrameY ; j++ )
			this->getFrameCircles( _tabCircles, i, j );
}

void XYGrid::getAllCircles(HoughSpace & _hs, float _rMax)
{
	for ( int i = 0 ; i < nbFrameX ; i++ )
		for ( int j = 0 ; j < nbFrameY ; j++ )
			this->getFrameCircles(_hs, _rMax, i, j );
}

void XYGrid::keepSlice(float _bas, float _haut)
{
	for ( int i = 0 ; i < nbFrameX ; i++ )
		for ( int j = 0 ; j < nbFrameY ; j++ )
			this->data[i][j].keepSlice( _bas, _haut );
}

void XYGrid::keepSlice(float _bas, int _nbPoints)
{
	for ( int i = 0 ; i < nbFrameX ; i++ )
		for ( int j = 0 ; j < nbFrameY ; j++ )
			this->data[i][j].keepSlice( _bas, _nbPoints );
}

void XYGrid::keepSlice(float _bas, float _haut, int _nbPoints)
{
	for ( int i = 0 ; i < nbFrameX ; i++ )
		for ( int j = 0 ; j < nbFrameY ; j++ )
			this->data[i][j].keepSlice( _bas, _haut, _nbPoints );
}

XYGrid XYGrid::getSlice(float _bas, float _haut)
{
	XYGrid rslt;
	
	rslt.setNbFrameX( nbFrameX );
	rslt.setNbFrameY( nbFrameY );
	
	for ( int i = 0 ; i < nbFrameX ; i++ )
	{
		rslt.data.push_back ( vector <GridFrame> () );
		for ( int j = 0 ; j < nbFrameY ; j++ )
		{
			rslt.data[i].push_back ( this->data[i][j].getSlice (_bas, _haut ) );
		}
	}
	
	return rslt;
}

XYGrid XYGrid::getSlice(float _bas, int _nbPoints)
{
	XYGrid rslt;
	
	rslt.setNbFrameX( nbFrameX );
	rslt.setNbFrameY( nbFrameY );
	
	for ( int i = 0 ; i < nbFrameX ; i++ )
	{
		rslt.data.push_back ( vector <GridFrame> () );
		for ( int j = 0 ; j < nbFrameY ; j++ )
		{
			rslt.data[i].push_back ( this->data[i][j].getSlice (_bas, _nbPoints) );
		}
	}
	
	return rslt;
}

XYGrid XYGrid::getSlice(float _bas, float _haut, int _nbPoints)
{
	XYGrid rslt;
	
	rslt.setNbFrameX( nbFrameX );
	rslt.setNbFrameY( nbFrameY );
	
	for ( int i = 0 ; i < nbFrameX ; i++ )
	{
		rslt.data.push_back ( vector <GridFrame> () );
		for ( int j = 0 ; j < nbFrameY ; j++ )
		{
			rslt.data[i].push_back ( this->data[i][j].getSlice (_bas, _haut, _nbPoints) );
		}
	}
	
	return rslt;
}

void XYGrid::printFrameBoundingSquare(int _i, int _j)
{
	cout<<"Boite englobante de la case ("<<_i<<","<<_j<<") : ";
	this->data[_i][_j].printBot();
	this->data[_i][_j].printTop();
}

void XYGrid::printBoundingBox()
{
	cout<<"Boite englobante de la grille : ";
	this->bot.printPoint();
	this->top.printPoint();
	cout<<endl;
}

void XYGrid::Sort()
{
	for(int i=0; i<nbFrameX; i++)
		for(int j=0; j<nbFrameY; j++)
			data[i][j].Sort();
}

float XYGrid::getFrameZMin(int _i, int _j)
{
	return this->data[_i][_j].getZMin();
}
